/***************************************************************************
 创建者: 华磊
 开始时间: 2018.12.13
 copyright: (C) 华友高科
 ***************************************************************************/
#include "laserdriverbeiyang.h"
#include "urg_node/urg_node_driver.h"


LaserDriverBeiyang::LaserDriverBeiyang(LaserDriverConfig configIn)
                                      :LaserDriverAbstract(configIn)
{
    isShutDown=false;
    initialDriver(configIn);
    createThread();
}

int LaserDriverBeiyang::initialDriver(LaserDriverConfig configIn)
{
    int tmp_port=atoi(configIn.port.c_str());
    laserDriver=new urg_node::UrgNode(configIn.hostname,tmp_port,configIn.intensity,4,
                                      configIn.frame_id,configIn.topicName,configIn.min_ang,configIn.max_ang);
    if(1!=laserDriver->initialDriver())
    {
        isDriverOk=false;
        return -1;
    }
//    laserDriver->run();
    return 1;
}

int LaserDriverBeiyang::publishLaserDataLoopOnce()
{
//    if(NULL!=laserDriver) 这样调用会延迟３ｓ,而且customtime 变成１３６ｍｓ
//    {
//        laserDriver->publishLaserDataLoopOnce();//发布频率太高，增加一次
//        return laserDriver->publishLaserDataLoopOnce();
//    }
//    return -1;
    return 1;
}

int LaserDriverBeiyang::shutDownDriver()
{
    qDebug()<<"shutDownDriver, LaserDriverBeiyang";
    isShutDown=true;
    if(NULL!=laserDriver)
    {
        delete laserDriver;
    }
    return 1;

    return 1;
}

bool LaserDriverBeiyang::isDriverStatusOk()
{
    return isDriverOk;
}

void* LaserDriverBeiyang::threadRun(void *)
{
    printf("globalManager theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 30;
    sched_setscheduler(0,SCHED_OTHER,&param);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    // 等待急停　di　模块初始化
    while(false==isShutDown)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
//         QThread::msleep(1000);
            if(NULL!=laserDriver)
            {
                laserDriver->publishLaserDataLoopOnce();
            }
//            if(NULL!=laserDriver)
//            {
//                laserDriver->scanThread();
//            }


    }
    printf("LaserDriverBeiyang thread quit!!!");
}

void* LaserDriverBeiyang::threadTask( void* classPtr )
{
    qDebug()<<"LaserDriverBeiyang thread run-------------------------";
    return ((LaserDriverBeiyang*)classPtr)->threadRun(NULL);
}

void LaserDriverBeiyang::createThread()
{
    int ret=pthread_create( &threadId, NULL, &LaserDriverBeiyang::threadTask,this);
    qDebug()<<"LaserDriverBeiyang createThread-------------------------";
}

